#ifndef MODEL_FORWARD_KINEMATICS_H
#define MODEL_FORWARD_KINEMATICS_H

#include <memory>


#include "basic_data_structure.hpp"
#include "robot_model.hpp"

namespace ZORAL
{
    class ForwardKinematics
    {
    private:
        std::shared_ptr<RobotModel> rm_;

    public:
        explicit ForwardKinematics(std::shared_ptr<RobotModel> rm)
        {
            rm_ = std::move(rm);
        }

        std::shared_ptr<RobotModel> getRobotModel()
        {
            return rm_;
        }

        int getRobotDof() const;
        int forwardPosition(const VectorXd &q, HomoTrans &T_out) const;
        int forwardPositions(const VectorXd &q, const bool &is_relate_to_base, std::vector<HomoTrans> &T_all, HomoTrans &T_out) const;
    };
} // namespace ZORAL

#endif